#include <iostream>
#include "Frame.h"
#include "Map.h"
#include "KeyFrame.h"
#include "MapPoint.h"
#include <opencv2/core/core.hpp>
#include <chrono>

#include "g2o/g2o/core/block_solver.h"
#include "g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/g2o/solvers/linear_solver_eigen.h"
#include "g2o/g2o/types/types_six_dof_expmap.h"
#include "g2o/g2o/core/robust_kernel_impl.h"
#include "g2o/g2o/solvers/linear_solver_dense.h"
#include "g2o/g2o/types/types_seven_dof_expmap.h"

#include "Converter.h"
#include <stdint.h>


#if 1
//Camera Parameters file
#define CAMERA_PARAMTERS_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC.yaml"

//image timestamp file
#define IMAGE_TIMESTAMP_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC_TimeStamps/V201.txt"

#define EUROC_DATA_LEFT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_01_easy/mav0/cam0/data"
#define EUROC_DATA_RIGHT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_01_easy/mav0/cam1/data"
#else
//Camera Parameters file
#define CAMERA_PARAMTERS_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC.yaml"

//image timestamp file
#define IMAGE_TIMESTAMP_FILE_PATH		"/home/wenyi/workspace/sift_slam/EuRoC_data_source/EuRoC_TimeStamps/V203.txt"

#define EUROC_DATA_LEFT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_03_difficult/mav0/cam0/data"
#define EUROC_DATA_RIGHT_FILE_PATH		"/home/wenyi/workspace/EuRoC/V2_03_difficult/mav0/cam1/data"
#endif

using namespace std;
using namespace cv;

void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &vTimeStamps);

int SearchByBruteForce(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches);

int PoseOptimization(Frame *pFrame);

void StereoInitialization();

bool TrackReferenceKeyFrame(int &getNmatches);

bool TrackLocalMap();

int SearchByProjection(Frame &F, const std::vector<MapPoint*> &vpMapPoints, const float th);

void CreateNewKeyFrame();


int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th);







Frame mCurrentFrame;
Frame mLastFrame;

//Local Map
KeyFrame* mpReferenceKF;
KeyFrame* mpLastKeyFrame;
std::vector<KeyFrame*> mvpLocalKeyFrames;
std::vector<MapPoint*> mvpLocalMapPoints;
unsigned int mnLastKeyFrameId;
bool init = false;

//Current matches in frame
int mnMatchesInliers;

float mThDepth;



Map* mpMap;

string currentImage;

int main()
{
	cout << "frame test" << endl;

	vector<string> vstrImageLeft;
	vector<string> vstrImageRight;
	vector<double> vTimeStamp;

	LoadImages(EUROC_DATA_LEFT_FILE_PATH, EUROC_DATA_RIGHT_FILE_PATH, IMAGE_TIMESTAMP_FILE_PATH, 
				vstrImageLeft, vstrImageRight, vTimeStamp);

	 if(vstrImageLeft.empty() || vstrImageRight.empty())
	 {
		cout << "load images failed" << endl;
		return -1;
	 }

	 if(vstrImageLeft.size()!=vstrImageRight.size())
	 {
		cout << "left image size: " << vstrImageLeft.size() <<" is not equal right image size: " << vstrImageRight.size() << endl;
		return -1;
	 }

	cv::FileStorage fsSettings(CAMERA_PARAMTERS_FILE_PATH, cv::FileStorage::READ);
	if(!fsSettings.isOpened())
	{
		cout << "open camera paramters file failed :" << CAMERA_PARAMTERS_FILE_PATH << endl;
		return -1;
	}

	cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    fsSettings["LEFT.K"] >> K_l;
    fsSettings["RIGHT.K"] >> K_r;

    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

	if(K_l.empty() || K_r.empty() ||
		P_l.empty() || P_r.empty() ||
		R_l.empty() || R_r.empty() ||
		D_l.empty() || D_r.empty() ||
		rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
	{
		cout << "Calibration parameters to rectify stereo are missing!" << endl;
	}


	cv::Mat mK;
    cv::Mat mDistCoef;
    float fx = fsSettings["Camera.fx"];
    float fy = fsSettings["Camera.fy"];
    float cx = fsSettings["Camera.cx"];
    float cy = fsSettings["Camera.cy"];

    cv::Mat K = cv::Mat::eye(3,3,CV_32F);
    K.at<float>(0,0) = fx;
    K.at<float>(1,1) = fy;
    K.at<float>(0,2) = cx;
    K.at<float>(1,2) = cy;
    K.copyTo(mK);

    cv::Mat DistCoef(4,1,CV_32F);
    DistCoef.at<float>(0) = fsSettings["Camera.k1"];
    DistCoef.at<float>(1) = fsSettings["Camera.k2"];
    DistCoef.at<float>(2) = fsSettings["Camera.p1"];
    DistCoef.at<float>(3) = fsSettings["Camera.p2"];
    const float k3 = fsSettings["Camera.k3"];
    if(k3!=0)
    {
        DistCoef.resize(5);
        DistCoef.at<float>(4) = k3;
    }
    DistCoef.copyTo(mDistCoef);

    float mbf = fsSettings["Camera.bf"];

    float fps = fsSettings["Camera.fps"];

	cout << "mb:" << mbf / fx << ", fps:" << fps << endl;


	mThDepth = mbf * (float)fsSettings["ThDepth"] / fx;

	cout << "mThDepth :" << mThDepth << endl;


	cv::Mat M1l,M2l,M1r,M2r;
    cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
    cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);



	cv::Mat imLeft, imRight, imLeftRect, imRightRect;
	

	//Create the Map
	mpMap = new Map();

	for(int i=0; i<vstrImageLeft.size(); i++)
	{
		currentImage = vstrImageLeft[i];
		// Read left and right images from file
        imLeft = cv::imread(vstrImageLeft[i],IMREAD_GRAYSCALE);
        imRight = cv::imread(vstrImageRight[i],IMREAD_GRAYSCALE);

		 if(imLeft.empty() || imRight.empty())
		 {
			cout << "read image faield" << endl;
			return -1;
		 }

		std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

		cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
		cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);

		mCurrentFrame = Frame(imLeftRect, imRightRect, vTimeStamp[i], mK, mDistCoef, mbf);

		//cout << "frame id:" << mCurrentFrame.m_frameId << endl;

		//init
		if(mCurrentFrame.m_keyPointsNumber > 100 && init == false)
		{
			StereoInitialization();
			continue;
		}

		if(init)
		{
			int getNmatches = 0;
			bool ret = TrackReferenceKeyFrame(getNmatches);

			mCurrentFrame.m_ReferenceKF = mpReferenceKF;

			if(ret == true)
			{
				//update mpReferenceKF
				ret = TrackLocalMap();
			}

			mLastFrame = Frame(mCurrentFrame);

			if(mLastFrame.m_frameId > mpReferenceKF->m_frameId + 2)
			{
				CreateNewKeyFrame();
			}
		}

		std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
		double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();
		//cout << "ttrack:" << ttrack * 1e3 << " ms" << endl;
	}
	return 0;
}



void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &vTimeStamps)
{
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImageLeft.reserve(5000);
    vstrImageRight.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png");
            vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            vTimeStamps.push_back(t/1e9);

        }
    }
}


int SearchByBruteForce(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches)
{
	int nmatches=0;
	const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();

    vpMapPointMatches = vector<MapPoint*>(F.m_keyPointsNumber, static_cast<MapPoint*>(NULL));
	
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce");
	std::vector<DMatch> matches;

	matcher->match(pKF->m_Descriptors, F.m_DescriptorsLeft, matches);

	vector<Point2f>  KFPoint, FPoint;
	for(size_t i=0; i<matches.size(); i++)
	{
		KFPoint.push_back(pKF->m_vectorKeyPoints[matches[i].queryIdx].pt);
		FPoint.push_back(F.m_vectorKeyPointsLeft[matches[i].trainIdx].pt);
	}

	vector<char> inLiners;
	Mat homography = findHomography(KFPoint, FPoint,inLiners, RANSAC, 30.0);

	if(homography.empty())
	{
		return 0;
	}
	else
	{
		for(size_t i=0; i<KFPoint.size(); i++)
		{
			if(inLiners[i] !=0)
			{
				MapPoint* pMP = vpMapPointsKF[matches[i].queryIdx];

                if(!pMP)
                    continue;

                if(pMP->isBadMapPoint())
                    continue;

				vpMapPointMatches[matches[i].trainIdx]=pMP;
				nmatches++;
			}
		}
	}

	return nmatches;
}



int PoseOptimization(Frame *pFrame)
{
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    int nInitialCorrespondences=0;

    // Set Frame vertex
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->m_RTcw));
    vSE3->setId(0);
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

    // Set MapPoint vertices
    const int N = pFrame->m_keyPointsNumber;

    vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
    vector<size_t> vnIndexEdgeMono;
    vpEdgesMono.reserve(N);
    vnIndexEdgeMono.reserve(N);

    vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
    vector<size_t> vnIndexEdgeStereo;
    vpEdgesStereo.reserve(N);
    vnIndexEdgeStereo.reserve(N);

    const float deltaMono = sqrt(5.991);
    const float deltaStereo = sqrt(7.815);


    {
    unique_lock<mutex> lock(MapPoint::g_GlobalMutex);

    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = pFrame->m_vectorMapPoints[i];
        if(pMP)
        {
            // Monocular observation
            if(pFrame->m_vectorURight[i]<0)
            {
                nInitialCorrespondences++;
                pFrame->m_vectorOutlier[i] = false;

                Eigen::Matrix<double,2,1> obs;
                const cv::KeyPoint &kpUn = pFrame->m_vectorKeyPointsLeftUn[i];
                obs << kpUn.pt.x, kpUn.pt.y;

                g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
                e->setMeasurement(obs);
                const float invSigma2 = 0.69f;
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(deltaMono);

                e->fx = pFrame->m_fx;
                e->fy = pFrame->m_fy;
                e->cx = pFrame->m_cx;
                e->cy = pFrame->m_cy;
                cv::Mat Xw = pMP->GetWorldPos();
                e->Xw[0] = Xw.at<float>(0);
                e->Xw[1] = Xw.at<float>(1);
                e->Xw[2] = Xw.at<float>(2);

                optimizer.addEdge(e);

                vpEdgesMono.push_back(e);
                vnIndexEdgeMono.push_back(i);
            }
            else  // Stereo observation
            {
                nInitialCorrespondences++;
                pFrame->m_vectorOutlier[i] = false;

                //SET EDGE
                Eigen::Matrix<double,3,1> obs;
                const cv::KeyPoint &kpUn = pFrame->m_vectorKeyPointsLeftUn[i];
                const float &kp_ur = pFrame->m_vectorURight[i];
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
                e->setMeasurement(obs);
                const float invSigma2 = 0.69f;
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                e->setInformation(Info);

                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(deltaStereo);

                e->fx = pFrame->m_fx;
                e->fy = pFrame->m_fy;
                e->cx = pFrame->m_cx;
                e->cy = pFrame->m_cy;
                e->bf = pFrame->m_baselineFx;
                cv::Mat Xw = pMP->GetWorldPos();
                e->Xw[0] = Xw.at<float>(0);
                e->Xw[1] = Xw.at<float>(1);
                e->Xw[2] = Xw.at<float>(2);

                optimizer.addEdge(e);

                vpEdgesStereo.push_back(e);
                vnIndexEdgeStereo.push_back(i);
            }
        }

    }
    }

	printf("nInitialCorrespondences:%d\n",nInitialCorrespondences);
    if(nInitialCorrespondences<3)
        return 0;

    // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
    // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
    const float chi2Mono[4]={5.991,5.991,5.991,5.991};
    const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};
    const int its[4]={10,10,10,10};    

    int nBad=0;
    for(size_t it=0; it<4; it++)
    {

        vSE3->setEstimate(Converter::toSE3Quat(pFrame->m_RTcw));
        optimizer.initializeOptimization(0);
        optimizer.optimize(its[it]);

        nBad=0;
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
        {
            g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

            const size_t idx = vnIndexEdgeMono[i];

            if(pFrame->m_vectorOutlier[idx])
            {
                e->computeError();
            }

            const float chi2 = e->chi2();

            if(chi2>chi2Mono[it])
            {                
                pFrame->m_vectorOutlier[idx]=true;
                e->setLevel(1);
                nBad++;
            }
            else
            {
                pFrame->m_vectorOutlier[idx]=false;
                e->setLevel(0);
            }

            if(it==2)
                e->setRobustKernel(0);
        }

        for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++)
        {
            g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];

            const size_t idx = vnIndexEdgeStereo[i];

            if(pFrame->m_vectorOutlier[idx])
            {
                e->computeError();
            }

            const float chi2 = e->chi2();

            if(chi2>chi2Stereo[it])
            {
                pFrame->m_vectorOutlier[idx]=true;
                e->setLevel(1);
                nBad++;
            }
            else
            {                
                e->setLevel(0);
                pFrame->m_vectorOutlier[idx]=false;
            }

            if(it==2)
                e->setRobustKernel(0);
        }

        if(optimizer.edges().size()<10)
            break;
    }    

    // Recover optimized pose and return number of inliers
    g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
    g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
    cv::Mat pose = Converter::toCvMat(SE3quat_recov);
    pFrame->SetPose(pose);

    return nInitialCorrespondences-nBad;
}



void StereoInitialization()
{
	// Set Frame pose to the origin
	mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));

	// Create KeyFrame
	KeyFrame* pKFini = new KeyFrame(mCurrentFrame, mpMap);
	
	// Create MapPoints and asscoiate to KeyFrame
	
	for(int i=0; i<mCurrentFrame.m_keyPointsNumber; i++)
	{
		float z = mCurrentFrame.m_vectorDepth[i];
		if(z > 0)
		{
			cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
			MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
			pNewMP->AddObservation(pKFini,i);
			pKFini->AddMapPoint(pNewMP,i);
			pNewMP->ComputeDistinctiveDescriptors();
			pNewMP->UpdateNormalAndDepth();
			mpMap->AddMapPoint(pNewMP);

			mCurrentFrame.m_vectorMapPoints[i] = pNewMP;
		}
	}
	
	cout << "New map created with " << mpMap->MapPointsSize() << " points" << endl;

	mLastFrame = Frame(mCurrentFrame);
	mnLastKeyFrameId = mCurrentFrame.m_frameId;

	mpLastKeyFrame = pKFini;

	mvpLocalKeyFrames.push_back(pKFini);
	mvpLocalMapPoints = mpMap->GetAllMapPoints();
	mpReferenceKF = pKFini;
	mCurrentFrame.m_ReferenceKF = pKFini;

	mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
	mpMap->m_vectorKeyFrameOrigins.push_back(pKFini);

	init = true;
}


bool TrackReferenceKeyFrame( int &getNmatches)
{
	//printf("mpReferenceKF id:%ld \n", mpReferenceKF->m_frameId);
	
	// We perform first an sift matching with the reference keyframe
	// If enough matches are found we setup a PnP solver
	vector<MapPoint*> vpMapPointMatches;
	mCurrentFrame.SetPose(mLastFrame.m_RTcw);
	fill(mCurrentFrame.m_vectorMapPoints.begin(),mCurrentFrame.m_vectorMapPoints.end(),static_cast<MapPoint*>(NULL));
	int nmatches = SearchByProjection(mCurrentFrame, mLastFrame, 7);

	getNmatches = nmatches;

	//cout <<"TrackReferenceKeyFrame nmatches:" << nmatches << endl;
	if(nmatches < 15)
	{
		cout << "frame id:" << mCurrentFrame.m_frameId << endl;
		cout << "mpReferenceKF kp size:" << mpReferenceKF->m_keyPointsNumber << ", mCurrentFrame kp size:" << mCurrentFrame.m_keyPointsNumber << endl;
		cout <<" SearchByBruteForce nmatches:" << nmatches << " < 5"  << ",vstrImageLeft[i] :"<< currentImage << endl;
		return false;
	}
	
	

	std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
	
	PoseOptimization(&mCurrentFrame);

	std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
	double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count();
	cout << "ttrack:" << ttrack * 1e3 << " ms" << endl;
	
	//Discard outliers
	int nmatchesMap = 0;
	for(int i =0; i<mCurrentFrame.m_keyPointsNumber; i++)
	{
		if(mCurrentFrame.m_vectorMapPoints[i])
		{
			if(mCurrentFrame.m_vectorOutlier[i])
			{
				MapPoint* pMP = mCurrentFrame.m_vectorMapPoints[i];
				mCurrentFrame.m_vectorMapPoints[i] = static_cast<MapPoint*>(NULL);
				mCurrentFrame.m_vectorOutlier[i] = false;
			}
			else if(mCurrentFrame.m_vectorMapPoints[i]->Observations()>0)
			{
				nmatchesMap++;
			}
		}
	}

	//cout << "TrackReferenceKeyFrame nmatchesMap : " << nmatchesMap << endl;
	if(nmatchesMap < 5)
	{
		cout << "frame id:" << mCurrentFrame.m_frameId << ",nmatches:" << nmatches <<  endl;
		cout << "mpReferenceKF kp size:" << mpReferenceKF->m_keyPointsNumber << ", mCurrentFrame kp size:" << mCurrentFrame.m_keyPointsNumber << endl;
		cout << "nmatchesMap : " << nmatchesMap << " < 5" << ",vstrImageLeft[i] :"<< currentImage << endl;
		return false;
	}

	return true;
}

void UpdateLocalKeyFrames();
void UpdateLocalPoints();

void SearchLocalPoints();


bool TrackLocalMap()
{
	//update local Map
	// This is for visualization
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

	//update mpReferenceKF
	UpdateLocalKeyFrames();
	UpdateLocalPoints();

	SearchLocalPoints();

	// Optimize Pose
    PoseOptimization(&mCurrentFrame);

	mnMatchesInliers = 0;

	// Update MapPoints Statistics
	for(int i=0; i<mCurrentFrame.m_keyPointsNumber; i++)
	{
		if(mCurrentFrame.m_vectorMapPoints[i])
		{
			if(!mCurrentFrame.m_vectorOutlier[i])
			{
				mCurrentFrame.m_vectorMapPoints[i]->IncreaseFound();
				mnMatchesInliers++;
			}
			else
			{
				mCurrentFrame.m_vectorMapPoints[i] = static_cast<MapPoint*>(NULL);
			}
		}
	}

	//cout << "TrackLocalMap mnMatchesInliers : " << mnMatchesInliers << endl;
	if(mnMatchesInliers < 5)
	{
		cout << "mnMatchesInliers : " << mnMatchesInliers << " < 5" << ",vstrImageLeft[i] :"<< currentImage << endl;
		return false;
	}

	return true;
}


void UpdateLocalKeyFrames()
{
	// Each map point vote for the keyframes in which it has been observed
	map<KeyFrame*,int> keyframeCounter;
	for(int i=0; i<mCurrentFrame.m_keyPointsNumber; i++)
	{
		if(mCurrentFrame.m_vectorMapPoints[i])
		{
			MapPoint* pMP = mCurrentFrame.m_vectorMapPoints[i];
			if(!pMP->isBadMapPoint())
			{
				const map<KeyFrame*,size_t> observations = pMP->GetObservations();
				for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), end=observations.end(); it!=end; it++)
				{
					keyframeCounter[it->first]++;
				}
			}
			else
			{
				mCurrentFrame.m_vectorMapPoints[i]=NULL;
			}
		}
	}


	//printf("----keyframeCounter size:%d\n", keyframeCounter.size());

	if(keyframeCounter.empty())
	{
		return;
	}

	int max=0;
	KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
	mvpLocalKeyFrames.clear();
	mvpLocalKeyFrames.reserve(3 * keyframeCounter.size());

	// All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
	for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), End=keyframeCounter.end(); it!=End; it++)
	{
		KeyFrame* pKF = it->first;
		 if(pKF->isBadKeyFrame())
		 {
			continue;
		 }

		 if(it->second > max)
		 {
			max = it->second;
			pKFmax = pKF;
		 }

		 mvpLocalKeyFrames.push_back(it->first);
		 pKF->m_TrackReferenceForFrame = mCurrentFrame.m_frameId;
	}

	// Include also some not-already-included keyframes that are neighbors to already-included keyframes
	for(vector<KeyFrame*>::const_iterator it=mvpLocalKeyFrames.begin(), End=mvpLocalKeyFrames.end(); it!=End; it++)
	{
		// Limit the number of keyframes
		if(mvpLocalKeyFrames.size()>80)
		{
			break;
		}

		KeyFrame* pKF = *it;
		const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
		for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
		{
			KeyFrame* pNeighKF = *itNeighKF;
			if(!pNeighKF->isBadKeyFrame())
			{
				if(pNeighKF->m_TrackReferenceForFrame != mCurrentFrame.m_frameId)
				{
					mvpLocalKeyFrames.push_back(pNeighKF);
					pNeighKF->m_TrackReferenceForFrame = mCurrentFrame.m_frameId;
					break;
				}
			}
		}

		const set<KeyFrame*> spChilds = pKF->GetChilds();
		for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
		{
			KeyFrame* pChildKF = *sit;
			if(!pChildKF->isBadKeyFrame())
			{
				if(pChildKF->m_TrackReferenceForFrame != mCurrentFrame.m_frameId)
				{
					 mvpLocalKeyFrames.push_back(pChildKF);
					 pChildKF->m_TrackReferenceForFrame = mCurrentFrame.m_frameId;
					 break;
				}
			}
			
		}
		
		KeyFrame* pParent = pKF->GetParent();
		if(pParent)
		{
			if(pParent->m_TrackReferenceForFrame != mCurrentFrame.m_frameId)
			{
				mvpLocalKeyFrames.push_back(pParent);
				pParent->m_TrackReferenceForFrame = mCurrentFrame.m_frameId;
				break;
			}
		}
	}

	 if(pKFmax)
	 {
		mpReferenceKF = pKFmax;
		mCurrentFrame.m_ReferenceKF = mpReferenceKF;
	 }
}


void UpdateLocalPoints()
{
	mvpLocalMapPoints.clear();
	for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
	{
		KeyFrame* pKF = *itKF;
		const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
		for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
		{
			MapPoint* pMP = *itMP;
			if(!pMP)
			{
				continue;
			}

			if(pMP->m_TrackReferenceForFrameId == mCurrentFrame.m_frameId)
			{
				continue;
			}

			if(!pMP->isBadMapPoint())
			{
				mvpLocalMapPoints.push_back(pMP);
				pMP->m_TrackReferenceForFrameId = mCurrentFrame.m_frameId;
			}
		}
	}

}


void SearchLocalPoints()
{
	// Do not search map points already matched
	for(vector<MapPoint*>::iterator vit=mCurrentFrame.m_vectorMapPoints.begin(), vend=mCurrentFrame.m_vectorMapPoints.end(); vit!=vend; vit++)
	{
		MapPoint* pMP = *vit;
		if(pMP)
		{
			 if(pMP->isBadMapPoint())
			 {
				*vit = static_cast<MapPoint*>(NULL);
			 }
			 else
			 {
				pMP->IncreaseVisible();
				pMP->m_LastFrameSeenId = mCurrentFrame.m_frameId;
				pMP->m_TrackInView = false;
			 }
		}
	}

	int nToMatch=0;
	// Project points in frame and check its visibility
	for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
	{
		MapPoint* pMP = *vit;
		if(pMP->m_LastFrameSeenId == mCurrentFrame.m_frameId)
		{
			continue;
		}

		if(pMP->isBadMapPoint())
		{
			continue;
		}

		// Project (this fills MapPoint variables for matching)
		if(mCurrentFrame.isInFrustum(pMP,0.5))
		{
			pMP->IncreaseVisible();
			nToMatch++;
		}
	}

	//cout << "SearchLocalPoints +++++nToMatch:" << nToMatch << ",mvpLocalMapPoints size:" << mvpLocalMapPoints.size() << endl;
	if(nToMatch>0)
	{
		SearchByProjection(mCurrentFrame, mvpLocalMapPoints, 10);
	}
}

float RadiusByViewingCos(const float &viewCos)
{
    if(viewCos>0.998)
        return 2.5;
    else
        return 4.0;
}

#define 	TH_HIGH    500
#define 	HISTO_LENGTH 30

int SearchByProjection(Frame &F, const std::vector<MapPoint*> &vpMapPoints, const float th)
{
	int nmatches=0;

	for(size_t iMP=0; iMP<vpMapPoints.size(); iMP++)
	{
		MapPoint* pMP = vpMapPoints[iMP];
		if(!pMP->m_TrackInView)
			continue;

		if(pMP->isBadMapPoint())
			continue;

		// The size of the window will depend on the viewing direction
		float r = RadiusByViewingCos(pMP->m_TrackViewCos);

		r = r * th;

		const vector<size_t> vIndices =
				F.GetFeaturesInArea(pMP->m_TrackProjX, pMP->m_TrackProjY, r);

		//printf("vIndices size:%d\n", vIndices.size());
	
		if(vIndices.empty())
		{
			continue;
		}
	
		const cv::Mat MPdescriptor = pMP->GetDescriptor();
	
		int bestDist = INT32_MAX;
		int bestDist2 = INT32_MAX;
		int bestIdx = -1 ;
	
		// Get best and second matches with near keypoints
		for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
		{
			const size_t idx = *vit;
	
			if(F.m_vectorMapPoints[idx])
			{
				if(F.m_vectorMapPoints[idx]->Observations() > 0)
				{
					continue;
				}
			}
	
			if(F.m_vectorURight[idx] > 0)
			{
				const float er = fabs(pMP->m_TrackProjXR - F.m_vectorURight[idx]);
				if(er > r)
				{
					continue;
				}
			}
	
			const cv::Mat &d = F.m_DescriptorsLeft.row(idx);
	
			const int dist = cv::norm(MPdescriptor,d);
	
			if(dist < bestDist)
			{
				bestDist2 = bestDist;
				bestDist = dist;
				bestIdx = idx;
			}
			else if(dist < bestDist2)
			{
				bestDist2 = dist;
			}
		}
	
		// Apply ratio to second match (only if best and second are in the same scale level)
		//printf("----bestDist:%d\n", bestDist);
		if(bestDist<=TH_HIGH)
		{
			F.m_vectorMapPoints[bestIdx] = pMP;
			nmatches++;
		}
	}

	//printf("SearchByProjection-----nmatches:%d\n", nmatches);
	return nmatches;
}

void CreateNewKeyFrame()
{
	KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap);
	mpReferenceKF = pKF;
	mCurrentFrame.m_ReferenceKF = pKF;

	mCurrentFrame.UpdatePoseMatrices();

	//We sort points by the measured depth by the stereo/RGBD sensor.
	// We create all those MapPoints whose depth < mThDepth.
	// If there are less than 100 close points we create the 100 closest.
	vector<pair<float,int> > vDepthIdx;
	vDepthIdx.reserve(mCurrentFrame.m_keyPointsNumber);

	for(int i=0; i<mCurrentFrame.m_keyPointsNumber; i++)
	{
		float z = mCurrentFrame.m_vectorDepth[i];
		if(z>0)
		{
			vDepthIdx.push_back(make_pair(z,i));
		}
	}

	//printf("++++++ vDepthIdx: %d\n", vDepthIdx.size());
	if(!vDepthIdx.empty())
	{
		sort(vDepthIdx.begin(),vDepthIdx.end());
		int nPoints = 0;

		for(size_t j=0; j<vDepthIdx.size();j++)
		{
			int i = vDepthIdx[j].second;
			bool bCreateNew = false;

			MapPoint* pMP = mCurrentFrame.m_vectorMapPoints[i];
			if(!pMP)
			{
				bCreateNew = true;
			}
			else if(pMP->Observations()<1)
			{
				bCreateNew = true;
				mCurrentFrame.m_vectorMapPoints[i] = static_cast<MapPoint*>(NULL);
			}

			if(bCreateNew)
			{
				cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
				MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
				pNewMP->AddObservation(pKF,i);
				pKF->AddMapPoint(pNewMP,i);
				pNewMP->ComputeDistinctiveDescriptors();
				pNewMP->UpdateNormalAndDepth();
				mpMap->AddMapPoint(pNewMP);

				mCurrentFrame.m_vectorMapPoints[i] = pNewMP;
				nPoints++;
			}
			else
			{
				nPoints++;
			}

			if(vDepthIdx[j].first > mThDepth && nPoints > 100)
			{
				//printf("-------------------------------vDepthIdx[j].first:%f\n", vDepthIdx[j].first);
				break;
			}
		}

		//printf("=============nPoints:%d\n", nPoints);
	}

	mnLastKeyFrameId = mCurrentFrame.m_frameId;
	mpLastKeyFrame = pKF;
}


void ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)
{
    int max1=0;
    int max2=0;
    int max3=0;

    for(int i=0; i<L; i++)
    {
        const int s = histo[i].size();
        if(s>max1)
        {
            max3=max2;
            max2=max1;
            max1=s;
            ind3=ind2;
            ind2=ind1;
            ind1=i;
        }
        else if(s>max2)
        {
            max3=max2;
            max2=s;
            ind3=ind2;
            ind2=i;
        }
        else if(s>max3)
        {
            max3=s;
            ind3=i;
        }
    }

    if(max2<0.1f*(float)max1)
    {
        ind2=-1;
        ind3=-1;
    }
    else if(max3<0.1f*(float)max1)
    {
        ind3=-1;
    }
}


int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th)
{
    int nmatches = 0;

	// Rotation Histogram (to check rotation consistency)
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);
    const float factor = 1.0f/HISTO_LENGTH;

    const cv::Mat Rcw = CurrentFrame.m_RTcw.rowRange(0,3).colRange(0,3);
    const cv::Mat tcw = CurrentFrame.m_RTcw.rowRange(0,3).col(3);

    const cv::Mat twc = -Rcw.t()*tcw;

    const cv::Mat Rlw = LastFrame.m_RTcw.rowRange(0,3).colRange(0,3);
    const cv::Mat tlw = LastFrame.m_RTcw.rowRange(0,3).col(3);

    const cv::Mat tlc = Rlw*twc+tlw;

    const bool bForward = tlc.at<float>(2)>CurrentFrame.m_baseline;
    const bool bBackward = -tlc.at<float>(2)>CurrentFrame.m_baseline;

    for(int i=0; i<LastFrame.m_keyPointsNumber; i++)
    {
        MapPoint* pMP = LastFrame.m_vectorMapPoints[i];

        if(pMP)
        {
            if(!LastFrame.m_vectorOutlier[i])
            {
                // Project
                cv::Mat x3Dw = pMP->GetWorldPos();
                cv::Mat x3Dc = Rcw*x3Dw+tcw;

                const float xc = x3Dc.at<float>(0);
                const float yc = x3Dc.at<float>(1);
                const float invzc = 1.0/x3Dc.at<float>(2);

                if(invzc<0)
                    continue;

                float u = CurrentFrame.m_fx * xc * invzc + CurrentFrame.m_cx;
                float v = CurrentFrame.m_fy * yc * invzc + CurrentFrame.m_cy;

                if(u<CurrentFrame.m_MinX || u>CurrentFrame.m_MaxX)
                    continue;
                if(v<CurrentFrame.m_MinY || v>CurrentFrame.m_MaxY)
                    continue;


                // Search in a window. Size depends on scale
                float radius = th * 1.2f;

                vector<size_t> vIndices2;

                if(bForward)
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius);
                else if(bBackward)
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius);
                else
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius);

                if(vIndices2.empty())
                    continue;

                const cv::Mat dMP = pMP->GetDescriptor();

                int bestDist = INT32_MAX;
                int bestIdx2 = -1;

                for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
                {
                    const size_t i2 = *vit;
                    if(CurrentFrame.m_vectorMapPoints[i2])
                        if(CurrentFrame.m_vectorMapPoints[i2]->Observations()>0)
                            continue;

                    if(CurrentFrame.m_vectorURight[i2]>0)
                    {
                        const float ur = u - CurrentFrame.m_baselineFx*invzc;
                        const float er = fabs(ur - CurrentFrame.m_vectorURight[i2]);
                        if(er>radius)
                            continue;
                    }

                    const cv::Mat &d = CurrentFrame.m_DescriptorsLeft.row(i2);

                    const int dist = cv::norm(dMP,d);

                    if(dist<bestDist)
                    {
                        bestDist=dist;
                        bestIdx2=i2;
                    }
                }

                if(bestDist<=TH_HIGH)
                {
                    CurrentFrame.m_vectorMapPoints[bestIdx2]=pMP;
                    nmatches++;
					if(1)
                    {
                        float rot = LastFrame.m_vectorKeyPointsLeftUn[i].angle - CurrentFrame.m_vectorKeyPointsLeftUn[bestIdx2].angle;
                        if(rot<0.0)
                            rot+=360.0f;
                        int bin = round(rot*factor);
                        if(bin==HISTO_LENGTH)
                            bin=0;
                        assert(bin>=0 && bin<HISTO_LENGTH);
                        rotHist[bin].push_back(bestIdx2);
                    }
                }
            }
        }
    }


	//Apply rotation consistency
    if(1)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            if(i!=ind1 && i!=ind2 && i!=ind3)
            {
                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
                {
                    CurrentFrame.m_vectorMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                    nmatches--;
                }
            }
        }
    }

    return nmatches;
}

